import network
import time
import _thread
import socket
import collections
from havi import ports

# ===== WiFi =====
SSID = "WiFi network name (Case sensitive)"
PASSWORD = "WiFi password "

wifi = network.WLAN(network.STA_IF)
wifi.active(True)
wifi.disconnect()
wifi.connect(SSID, PASSWORD)

print("Connecting to WiFi...")
for _ in range(20):
    if wifi.isconnected():
        break
    print(".", end="")
    time.sleep(1)
print()
if wifi.isconnected():
    print("Connected:", wifi.ifconfig())
else:
    print("Failed to connect to WiFi")

# ===== Hardware =====
servo = ports.servo1()

# Two ultrasonic sensors using Havi ports
ultrasonic1 = ports.input1("ultrasonic sensor")   # front
ultrasonic2 = ports.input2("ultrasonic sensor")   # back

# ===== Shared state =====
data_queue = collections.deque((), 60)   # buffer
sweep_delay = 0.03
MAX_RANGE = 100

# ===== Servo sweep thread =====
def servo_sweep():
    global data_queue
    min_angle, max_angle = 5, 175
    step, direction = 1, 1
    while True:
        for ang in range(min_angle, max_angle + 1, step):
            if direction == -1:
                ang = max_angle - (ang - min_angle)
            try:
                servo.move(ang)
            except:
                pass

            # Read both sensors
            try:
                d1 = ultrasonic1.distance_cm()
            except:
                d1 = None
            try:
                d2 = ultrasonic2.distance_cm()
            except:
                d2 = None

            # Map sensor 1 (front) to 0–180°
            if d1 is not None and 2 < d1 <= MAX_RANGE:
                data_queue.append((ang, int(d1), 1, 0))   # valid
            elif d1 is not None and d1 > MAX_RANGE:
                data_queue.append((ang, MAX_RANGE, -1, 0))  # out of range
            else:
                data_queue.append((ang, 0, 2, 0))           # no reading

            # Map sensor 2 (back) to 180–360°
            back_ang = (ang + 180) % 360
            if d2 is not None and 2 < d2 <= MAX_RANGE:
                data_queue.append((back_ang, int(d2), 1, 1))  # valid
            elif d2 is not None and d2 > MAX_RANGE:
                data_queue.append((back_ang, MAX_RANGE, -1, 1))
            else:
                data_queue.append((back_ang, 0, 2, 1))

            time.sleep(sweep_delay)
        direction *= -1

# start sweep in background
_thread.start_new_thread(servo_sweep, ())

# ===== HTML UI =====
html = """<!doctype html>
<html><head><meta charset="utf-8"><meta name="viewport" content="width=device-width,initial-scale=1">
<title>SONAR 360°</title>
<style>
body{background:#000;color:#9ff49a;font-family:monospace;text-align:center;position:relative}
canvas{display:block;margin:12px auto;}
#info{margin-top:6px}
#statusdot{width:14px;height:14px;border-radius:50%;display:inline-block;margin-left:8px;background:#555;}
#ctrl{margin-top:10px;color:#9ff49a}
#legend{position:absolute;left:20px;top:450px;text-align:left;color:#9ff49a;font-size:14px;}
#legend div{margin:4px 0;}
input[type=range]{width:200px}
</style>
</head><body>
<h2>SONAR 360° <span id="statusdot"></span></h2>
<div style="position:relative;width:520px;margin:auto;">
  <canvas id="rad" width="520" height="520"></canvas>
  <!-- Legend -->
  <div id="legend">
    <div><span style="color:#0f0;">●</span> Front Sensor</div>
    <div><span style="color:#0ff;">●</span> Back Sensor</div>
    <div><span style="color:#f00;">●</span> Error</div>
    <div><span style="color:#00f;">●</span> No Reading</div>
  </div>
</div>

<div id="info">
  Angle: --°<br>
  Front: -- cm | Back: -- cm
</div>
<div id="ctrl">
Sweep delay: <span id="delayval">30</span> ms<br>
<input type="range" id="delay" min="5" max="200" value="30">
</div>
<script>
const c=document.getElementById('rad'),ctx=c.getContext('2d'),W=c.width,H=c.height,CX=W/2,CY=H/2,R=Math.min(W,H)*0.42,MAX=100;
const dot=document.getElementById('statusdot');
let lastUpdate=Date.now();
let points=[];let sweepAngle=0;let sweepDist=MAX;
let frontDist="--"; let backDist="--";

function drawBg(){
  ctx.fillStyle='rgba(0,0,0,0.25)';ctx.fillRect(0,0,W,H);
  ctx.font='14px monospace';
  ctx.lineWidth=1.5;
  for(let i=1;i<=4;i++){
    let r=R*(i/4);
    ctx.beginPath();
    ctx.arc(CX,CY,r,0,2*Math.PI);
    ctx.strokeStyle='rgba(0,255,160,0.25)';
    ctx.lineWidth=2;
    ctx.stroke();
    ctx.fillStyle='rgba(160,255,200,0.95)';
    ctx.fillText((25*i)+' cm',CX+6,CY-r+16);
  }
  for(let a=0;a<360;a+=30){
    let rad=a*Math.PI/180,x=CX+Math.cos(rad)*R,y=CY+Math.sin(rad)*R;
    ctx.beginPath();
    ctx.moveTo(CX,CY);
    ctx.lineTo(x,y);
    ctx.strokeStyle='rgba(0,255,120,0.15)';
    ctx.stroke();
    if(a%90===0)ctx.fillText(a+'°',x,y);
  }
  ctx.beginPath();ctx.arc(CX,CY,4,0,2*Math.PI);ctx.fill();
}
function drawSweep(){
  let rad=sweepAngle*Math.PI/180;
  let r=(sweepDist/MAX)*R;
  let x=CX+Math.cos(rad)*r,y=CY+Math.sin(rad)*r;
  ctx.lineWidth=1.2;
  ctx.strokeStyle='rgba(0,255,0,0.6)';
  ctx.beginPath();
  ctx.moveTo(CX,CY);
  ctx.lineTo(x,y);
  ctx.stroke();
}
function drawBlips(){
  let now=Date.now();
  points=points.filter(p=>now-p.t<4000);
  for(let p of points){
    let age=(now-p.t)/4000;
    let alpha=1-age;

    if(p.ok===1){
      ctx.fillStyle = (p.side===0) 
        ? 'rgba(0,255,0,'+alpha+')'   // front = green
        : 'rgba(0,255,255,'+alpha+')';// back = cyan
    }
    else if(p.ok===0){ ctx.fillStyle='rgba(255,0,0,'+alpha+')'; }
    else if(p.ok===2){ ctx.fillStyle='rgba(0,0,255,'+alpha+')'; }
    else { continue; }

    ctx.beginPath();
    ctx.arc(p.x,p.y,5,0,2*Math.PI);
    ctx.fill();
  }
}
function addReading(angle,dist,ok,side){
  sweepAngle=angle;
  sweepDist=(ok===1)?dist:MAX;
  let safe=Math.max(0,Math.min(dist||0,MAX));
  let rad=angle*Math.PI/180;
  let r=(safe/MAX)*R;
  let x=CX+Math.cos(rad)*r,y=CY+Math.sin(rad)*r;
  if(ok !== -1){
    points.push({x,y,t:Date.now(),ok:ok,side:side});
  }

  // update distances
  if(side === 0){ frontDist = (ok===1)? dist+" cm" : "--"; }
  if(side === 1){ backDist  = (ok===1)? dist+" cm" : "--"; }

  document.getElementById('info').innerHTML =
    "Angle: "+angle+"°<br>Front: "+frontDist+" | Back: "+backDist;

  lastUpdate=Date.now();
  drawFrame();
}
function drawFrame(){drawBg();drawSweep();drawBlips();}
async function poll(){
  try{
    let res=await fetch('/data');
    let t=await res.text();
    let pairs=t.split(";");
    for(let p of pairs){
      if(!p) continue;
      let [a,d,ok,side]=p.split(",");
      addReading(parseInt(a)||0,parseInt(d)||0,parseInt(ok)||0,parseInt(side)||0);
    }
  }catch(e){}
  setTimeout(poll,60);
}
setInterval(()=>{if(Date.now()-lastUpdate>200){dot.style.background='#f00';}},100);
drawBg();poll();

// sweep delay control
const slider=document.getElementById('delay'),disp=document.getElementById('delayval');
slider.addEventListener('input',async e=>{
  let v=slider.value;
  disp.innerText=v;
  try{await fetch('/speed?val='+v);}catch(err){}
});
</script>
</body></html>
"""

# ===== Simple HTTP server =====
addr = socket.getaddrinfo("0.0.0.0", 80)[0][-1]
s = socket.socket()
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(addr)
s.listen(1)
s.settimeout(0.2)
print("HTTP server listening on", addr)

while True:
    try:
        try:
            cl, a = s.accept()
        except OSError:
            continue
        req = cl.recv(1024).decode()
        if req.startswith("GET /data"):
            if len(data_queue) >= 2:
                d1 = data_queue.popleft()
                d2 = data_queue.popleft()
            elif data_queue:
                d1 = data_queue.popleft()
                d2 = (0, 0, 2, 1)
            else:
                d1 = (0, 0, 2, 0)
                d2 = (0, 0, 2, 1)

            cl.send("HTTP/1.0 200 OK\r\nContent-type: text/plain\r\n\r\n")
            cl.send("%d,%d,%d,%d;%d,%d,%d,%d" % (d1[0],d1[1],d1[2],d1[3],
                                                 d2[0],d2[1],d2[2],d2[3]))
            cl.close()
        elif req.startswith("GET /speed"):
            try:
                val = req.split("val=")[1].split(" ")[0]
                ms = max(5, min(200, int(val)))
                sweep_delay = ms / 1000.0
                print("Sweep delay set to", sweep_delay, "seconds")
                cl.send("HTTP/1.0 200 OK\r\nContent-type: text/plain\r\n\r\nOK")
            except Exception as e:
                print("Speed change error:", e)
                cl.send("HTTP/1.0 400 Bad Request\r\n\r\nERR")
            cl.close()
        else:
            cl.send("HTTP/1.0 200 OK\r\nContent-type: text/html\r\n\r\n")
            cl.send(html)
            cl.close()
    except Exception as e:
        print("HTTP server loop error:", e)

